package crtl;

import java.util.HashMap;
import java.util.Map;
import java.util.Properties;


/**
 *
 * @author marcel meraner
 */
public class BangBangFollowLeftWallCtrl extends BangBangCtrl {
	
	private final static String PROP_THRESHOLD_WALL_IN_FRONT = "ctrl.bangbang.followwall.threshold_wall_in_front";
	private final static String PROP_MIN_DISTANCE_VALUE = "ctrl.bangbang.followwall.min_dsensor_value";
	private final static String PROP_LOST_WALL_STEPS_BEFORE_TURN = "ctrl.bangbang.followwall.lost_wall_steps_before_turn";
	private final static String PROP_LEFT_WALL_GAP_MAX = "ctrl.bangbang.followwall.left_wall_gap_max";
	private final static String PROP_LEFT_WALL_GAP_MIN = "ctrl.bangbang.followwall.left_wall_gap_min";
	
	private final int _minDSensorValue = 100;
	private final int _thresholdWallInFront = 700;
	private final int _lostWallStepsBeforeTurn = 700;
	private final int _leftWallGapMin = 300;
	private final int _leftWallGapMax = 500;
	private final int _leftWallGapMid = _leftWallGapMin + ((_leftWallGapMax - _leftWallGapMin) / 2);
	
	private boolean _finished = false;
	private double _lastMax = 0;
	private int _lostCounter = 0;

	public BangBangFollowLeftWallCtrl() {
		super();

		System.out.println("BangBang - Follow Left Wall Controller initialized");
	}
	
	@Override
	protected boolean finished() {
		return _finished;
	}

	@Override
	protected Speed navigate() {
		double[] s = readDistanceSensors();
		s = calcAverage(s);
		
		HashMap<Direction, Double> values = convertSensorValues(s);
		double max = Double.MIN_VALUE;
		Direction d = Direction.FORWARD;
		
		for (Map.Entry<Direction, Double> entry : values.entrySet()) {
			if (entry.getValue() > max) {
				d = entry.getKey();
				max = entry.getValue();
			}
		}
		
		//System.out.print("max: " + max + " " + d);
		
		double intensity = 1.0;
		
		if (max < _minDSensorValue) {
			if (_lastMax > _minDSensorValue) {
				// Lost wall
				if (_lostCounter < _lostWallStepsBeforeTurn) {
					// First drive forwad
					d = Direction.FORWARD;
					_lostCounter++;
					
				} else {
					// After a while, turn hard left
					d = Direction.S_REAR_LEFT;
					
				}
				
			} else {
				// Nothing around - drive forward
				d = Direction.FORWARD;
				_lastMax = max;
				_lostCounter = 0;
			}
			
		} else if (values.get(Direction.S_FRONT_LEFT) > _thresholdWallInFront || values.get(Direction.S_FRONT_RIGHT) > _thresholdWallInFront) {
			// Wall in front, turn hard right
			d = Direction.S_REAR_RIGHT;
			intensity = 0.5;
			
			_lastMax = max;
			_lostCounter = 0;
			
		} else {
			
			switch (d) {
				
				case S_REAR_LEFT:
					d = Direction.S_FORWARD_LEFT;
					break;
					
				case S_LEFT:
					// Balacing along the left wall
					d = Direction.FORWARD;
					if (max > _leftWallGapMax) {
						d = Direction.S_RIGHT;
						
					} else if (max > _leftWallGapMid) {
						d = Direction.S_FRONT_RIGHT;
						
					} else if (max < _leftWallGapMin) {
						d = Direction.S_FRONT_LEFT;
						
					}
					break;
					
				case S_FORWARD_LEFT:
					if (max > _thresholdWallInFront) {
						// Wall in "front"
						d = Direction.S_REAR_RIGHT;
						intensity = 0.5;
					} else {
						// Balacing along the left wall
						d = Direction.S_RIGHT;
					}
					break;
					
				default:
					d = Direction.FORWARD;
					
			}
			
			_lostCounter = 0;
			_lastMax = max;
		}
		
		return calcSpeed(d, intensity);
	}
	
	public static void main(String[] args) {
		BangBangFollowLeftWallCtrl bbflwc = new BangBangFollowLeftWallCtrl();
		bbflwc.run();
	}
	
}
